// learn/imu.c

//线性回归等方法计算比例因子和非正交性

#include <stdio.h>
#include <stdlib.h>

// 假设我们有 IMU 的旋转数据和对应的输出
#define DATA_POINTS 4

// 线性回归计算比例因子和截距
void linear_regression(double x[][2], double y[], double *scaling_factor, double *intercept) {
    double sum_x1 = 0, sum_x2 = 0, sum_y = 0;
    double sum_x1y = 0, sum_x2y = 0, sum_x1x1 = 0, sum_x2x2 = 0;

    for (int i = 0; i < DATA_POINTS; i++) {
        sum_x1 += x[i][0];
        sum_x2 += x[i][1];
        sum_y += y[i];
        sum_x1y += x[i][0] * y[i];
        sum_x2y += x[i][1] * y[i];
        sum_x1x1 += x[i][0] * x[i][0];
        sum_x2x2 += x[i][1] * x[i][1];
    }

    // 计算比例因子
    *scaling_factor = (sum_x1y + sum_x2y) / (sum_x1 + sum_x2);
    *intercept = (sum_y - (*scaling_factor) * (sum_x1 + sum_x2)) / DATA_POINTS;
}

// 计算非正交性
double calculate_non_orthogonality(double scaling_factor) {
    return scaling_factor - 1.0; // 假设理想比例因子为 1.0
}

int main() {
    // IMU 旋转数据
    double data_x[DATA_POINTS][2] = {
        {1, 0},
        {0, 1},
        {1, 1},
        {0, 0}
    };
    // IMU 输出
    double data_y[DATA_POINTS] = {1.1, 0.9, 1.0, 0.95};

    double scaling_factor, intercept;

    // 进行线性回归
    linear_regression(data_x, data_y, &scaling_factor, &intercept);

    // 输出结果
    printf("比例因子: %f\n", scaling_factor);
    printf("截距: %f\n", intercept);

    // 计算非正交性
    double non_orthogonality = calculate_non_orthogonality(scaling_factor);
    printf("非正交性: %f\n", non_orthogonality);

    return 0;
}
/*
1, 数据准备：data_x 是 IMU 的旋转数据，data_y 是对应的输出。你可以根据实际情况替换这些数据。
2. 线性回归：linear_regression 函数计算比例因子和截距。
3. 非正交性计算：calculate_non_orthogonality 函数简单地通过计算比例因子与理想值的偏差来估算非正交性。
4. 主函数：在 main 函数中调用线性回归和非正交性计算，并输出结果。
*/